/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
          /*LINTLIBRARY*/
          /*PROTOLIB1*/

#include <math.h>
#include <stdio.h>
#include "main.h"
#include "lsftopc.h"

/*************************************************************************
*
* NAME
*	LSFtoPC 
*
* FUNCTION
*
*	convert line spectral frequencies to predictor coefficients
*
* SYNOPSIS
*
*	LSFtoPC(f, pc, frame_num)
* 
*   formal
*			data	I/O
*	name		type	type	function
*	-------------------------------------------------------------------
*	f		real	i	lsp frequencies
*	pc		real	o	LPC predictor coefficients
*	frame_num	int	i	Frame number
*
*
***************************************************************************
*
* DESCRIPTION
*
*	LSFtoPC converts line spectral frequencies to predictor 
*	coefficients.  
*
*	The analysis filter may be reconstructed:
*
*		A(z) = 1/2 [ P(z) + Q(z) ]
*
*	CELP's LPC predictor coefficient convention is:
*              p+1         -(i-1)
*       A(z) = SUM   a   z          where a  = +1.0
*              i=1    i                    1
*
***************************************************************************/
void LSFtoPC(
float	lsf[ORDER],
float	pc[ORDER+1],
int	frame_num)
{
int 	i, j;
float	pi, xf, xx;
int	half_order;
float	a[ORDER/2 +1], a1[ORDER/2 +1], a2[ORDER/2 +1];
float	b[ORDER/2 +1], b1[ORDER/2 +1], b2[ORDER/2 +1];
float	p[ORDER/2], q[ORDER/2];
float	freq[ORDER];


/*  Check input for ill-conditioned cases */
	if( (lsf[0] <= 0.0) || (lsf[0] >= 0.5) )	{
	  printf("LSFtoPC: LSFs out of bounds; lsf[0] = %1.4f at frame %d\n",
		lsf[0], frame_num);
	}

	for(i=1; i<ORDER; i++)	{
	  if(lsf[i] <= lsf[i-1])
	    printf("LSFtoPC: nonmonotonic LSFs at frame %d\n", frame_num);
	  if( (lsf[i] <= 0.0) || (lsf[i] >= 0.5) )
	    printf("LSFtoPC: LSFs out of bounds; lsf[%d] = %1.4f at frame %d\n",
		i, lsf[i], frame_num);
	}

/*  Initialization */
/*	pi = 4*atan(1.0);*/
  	pi = 3.1415926535897931032;

	for(i=0;i<ORDER;i++)	
	  freq[i] = lsf[i];

	half_order = ORDER/2;

	for(i=0;i<=half_order;i++)	{
	  a[i] = 0.0;
	  a1[i] = 0.0;
	  a2[i] = 0.0;
	  b[i] = 0.0;
	  b1[i] = 0.0;
	  b2[i] = 0.0;
	}

/*  LSF filter parameters */
	for(i=0;i<half_order;i++)
	{
/*	  p[i] = -2.0 * (float)cos( 2.0 * pi * (double)freq[2*i] );
	  q[i] = -2.0 * (float)cos( 2.0 * pi * (double)freq[2*i+1] );*/
    	  p[i] = -2. * cos(2. * pi * freq[2 * i]);
    	  q[i] = -2. * cos(2. * pi * freq[2 * i + 1]);

	}

/*  Impulse response of analysis filter */
	xf = 0.0;
	for(i=0;i<=ORDER;i++)	{

	  xx = 0.0;
	  if (i == 0)
	    xx = 1.0;
	  a[0] = xx + xf;
	  b[0] = xx - xf;
	  xf = xx;

	  for(j=0;j<half_order;j++)	{
	    a[j+1] = a[j] + p[j] * a1[j] + a2[j];
	    b[j+1] = b[j] + q[j] * b1[j] + b2[j];
	    a2[j] = a1[j];
	    a1[j] = a[j];
	    b2[j] = b1[j];
	    b1[j] = b[j];
	  }
	
	  if(i > 0)
	    pc[i-1] = -0.5 * (a[half_order] + b[half_order]);

	}	

/*  Convert to CELPs PC array configuration */
	for(i=9;i>=0;i--)	
	  pc[i+1] = -pc[i];
	pc[0] = 1.0;


}
